#include "control.h"

#include "main.h"
#include "stdio.h"

uint8_t run_flag = 0;

float target_angle = -1.091;
float target_turn  = 0;

float motor_curPwmL = 0;
float motor_curPwmR = 0;

float pid_speedKp = 0.1;
float pid_speedKi = 0.1 / 200;
// float pid_speedKd = 0;

float pid_standKp = 280 * 0.6;
// float pid_standKi = 0;
float pid_standKd = 2.25 * 0.6;

float pid_turnKp = 10;
// float pid_turnKi = 0;
float pid_turnKd = 0.6;


float pitch = 0, roll = 0, yaw = 0;
short gyrox = 0, gyroy = 0, gyroz = 0;
// short aacx = 0, aacy = 0, aacz = 0;

// Pid_StructDef pidInfo_SpeedL = {0};
// Pid_StructDef pidInfo_SpeedR = {0};

PidSpeedInfo_StructDef pidInfo_Speed = {0};

void host_cmdDeal(Host_CmdStructDef cmd)
{
    switch (cmd.cmd) {
    case CMD_SPEED_TARGET_Whole:
        pidInfo_Speed.target = cmd.data;
        break;
    // case CMD_SPEED_TARGET_LEFT:
    //     pidInfo_SpeedL.target = cmd.data;
    //     break;
    // case CMD_SPEED_TARGET_RIGHT:
    //     pidInfo_SpeedR.target = cmd.data;
    //     break;
    // case CMD_SPEED_PID_KP:
    //     pid_speedKp = cmd.data;
    //     break;
    // case CMD_SPEED_PID_KI:
    //     pid_speedKi = cmd.data;
    //     break;
    // case CMD_SPEED_PID_KD:
    //     pid_speedKd = cmd.data;
    //     break;

    case CMD_STAND_PID_KP:
        pid_standKp = cmd.data;
        break;
    // case CMD_STAND_PID_KI:
    //     pid_standKi = cmd.data;
    //     break;
    case CMD_STAND_PID_KD:
        pid_standKd = cmd.data;
        break;

    // case CMD_PWM_L:
    //     motor_curPwmL = cmd.data;
    //     break;
    // case CMD_PWM_R:
    //     motor_curPwmR = cmd.data;
    //     break;

    case CMD_RUN:
        run_flag = 1;
        break;
    case CMD_STOP:
        run_flag = 0;
        break;

    case CMD_SET_ANGLE:
        target_angle = cmd.data;
        break;

    // case CMD_MUT06:
    //     pid_standKp *= 0.6;
    //     pid_standKd *= 0.6;
    //     break;

    case CMD_SET_TURN:
        target_turn = cmd.data;
        break;

    case CMD_TURN_PIK_KP:
        pid_turnKp = cmd.data;
        break;
    // case CMD_TURN_PIK_KI:
    //     pid_turnKi = cmd.data;
    //     break;
    case CMD_TURN_PIK_KD:
        pid_turnKd = cmd.data;
        break;

    default:
        break;
    }
}
